//
// Created by Sonny on 2023/10/4.
//

#ifndef M2006_RE_ALG_PID_H
#define M2006_RE_ALG_PID_H
#include <cstdint>

/*PID计算模式*/
enum Enum_PID_Mod{
    PID_Position = 0,
    PID_Delta,
};

class PID{
public:

    void PID_SET(float kp,float ki,float kd,float kf);
    void PID_ModInit(uint8_t mode = PID_Position, uint8_t expert = 0, float i_OUT_MAX = 0.0f, float oUT_MAX = 0.0f, float dead_Zone = 0.0f);//PID模式选择(1=计算模式,2=专家PID开关,3=积分限幅,4=输出限幅,5=死区(在专家模式需开启))
    void PID_IMAX();
    void PID_DeadZone();
    void PID_OUTMAX();
    void PID_FeedForward();
    void PID_Calc();

    float Kp,Ki,Kd,Kf;
    /*0-Now 1-Last 2-LLast*/
    float Now[2];
    float Target[2];
    float Error[3];
    // P输出
    float p_out = 0.0f;
    // I输出
    float i_out[2];
    // D输出
    float d_out = 0.0f;
    // F输出
    float f_out = 0.0f;
    //总输出
    float OUt[2];

    /*专家PID*/
    uint8_t Mode = 0;
    uint8_t Expert = 0;
    float Delta_Error[2];
    float Low_Error;
    float Mid_Error;
    float High_Error;
    /*PID_Mod*/
    float I_OUT_MAX = 0.0f;
    float OUT_MAX = 0.0f;
    float Dead_Zone = 0.0f;

    uint8_t Test_start = 0;//测试模式标志位,由转口置一开始
};


#endif //M2006_RE_ALG_PID_H
